#!/usr/bin/env python3
"""
EchoKit Message Handler - 处理EchoKit服务器消息格式转换
解决LLM返回格式与机器人客户端期望格式不匹配的问题
"""

import re
import json
import logging
import time
from typing import Dict, Any, Optional, Tuple
from geometry_msgs.msg import Twist


class EchoKitMessageHandler:
    """处理EchoKit服务器消息的格式转换和命令解析"""
    
    def __init__(self, logger=None):
        self.logger = logger or logging.getLogger(__name__)
        
        # 命令映射表 - 将方括号命令映射到具体动作
        self.command_mapping = {
            'forward': {'linear': 0.3, 'angular': 0.0},
            'backward': {'linear': -0.3, 'angular': 0.0},
            'turn_left': {'linear': 0.0, 'angular': 1.0},
            'turn_right': {'linear': 0.0, 'angular': -1.0},
            'left': {'linear': 0.0, 'angular': 1.0},
            'right': {'linear': 0.0, 'angular': -1.0},
            'stop': {'linear': 0.0, 'angular': 0.0},
        }
        
        # 方括号命令的正则表达式
        self.bracket_pattern = re.compile(r'\[([^\]]+)\]')
        
    def parse_message(self, raw_message: str) -> Optional[Dict[str, Any]]:
        """
        解析来自EchoKit服务器的原始消息
        
        Args:
            raw_message: 原始消息字符串
            
        Returns:
            解析后的消息字典，如果解析失败返回None
        """
        try:
            # 首先尝试解析为JSON
            try:
                json_data = json.loads(raw_message)
                return self._handle_json_message(json_data)
            except json.JSONDecodeError:
                pass
            
            # 如果不是JSON，尝试解析方括号格式
            bracket_match = self.bracket_pattern.search(raw_message)
            if bracket_match:
                command = bracket_match.group(1).lower().strip()
                return self._handle_bracket_command(command, raw_message)
            
            # 如果都不匹配，尝试直接文本解析
            return self._handle_text_message(raw_message)
            
        except Exception as e:
            if self.logger:
                self.logger.error(f"Error parsing message: {e}")
            return None
    
    def _handle_json_message(self, json_data: Dict[str, Any]) -> Dict[str, Any]:
        """处理JSON格式的消息"""
        # 如果JSON中包含命令信息
        if 'command' in json_data:
            command = json_data['command'].lower().strip()
            return {
                'type': 'voice_command',
                'command': command,
                'action': self.command_mapping.get(command),
                'original_message': json_data,
                'format': 'json'
            }
        
        # 如果JSON中包含文本内容，尝试从中提取命令
        if 'content' in json_data or 'text' in json_data:
            text = json_data.get('content', json_data.get('text', ''))
            bracket_match = self.bracket_pattern.search(text)
            if bracket_match:
                command = bracket_match.group(1).lower().strip()
                return {
                    'type': 'voice_command',
                    'command': command,
                    'action': self.command_mapping.get(command),
                    'original_message': json_data,
                    'format': 'json_with_bracket'
                }
        
        # 返回原始JSON消息
        return {
            'type': json_data.get('type', 'unknown'),
            'original_message': json_data,
            'format': 'json'
        }
    
    def _handle_bracket_command(self, command: str, raw_message: str) -> Dict[str, Any]:
        """处理方括号格式的命令"""
        return {
            'type': 'voice_command',
            'command': command,
            'action': self.command_mapping.get(command),
            'original_message': raw_message,
            'format': 'bracket'
        }
    
    def _handle_text_message(self, text: str) -> Dict[str, Any]:
        """处理纯文本消息，尝试从中识别命令"""
        text_lower = text.lower().strip()
        
        # 尝试匹配已知命令
        for cmd, action in self.command_mapping.items():
            if cmd in text_lower:
                return {
                    'type': 'voice_command',
                    'command': cmd,
                    'action': action,
                    'original_message': text,
                    'format': 'text'
                }
        
        # 如果没有匹配到命令，返回文本消息
        return {
            'type': 'text_message',
            'content': text,
            'original_message': text,
            'format': 'text'
        }
    
    def create_twist_message(self, action: Dict[str, float]) -> Twist:
        """根据动作字典创建Twist消息"""
        twist = Twist()
        if action:
            twist.linear.x = action.get('linear', 0.0)
            twist.angular.z = action.get('angular', 0.0)
        return twist
    
    def is_valid_command(self, command: str) -> bool:
        """检查命令是否有效"""
        return command.lower().strip() in self.command_mapping
    
    def get_available_commands(self) -> list:
        """获取所有可用命令列表"""
        return list(self.command_mapping.keys())
    
    def format_response_for_echokit(self, message_type: str, content: str) -> str:
        """格式化发送给EchoKit服务器的响应消息"""
        response = {
            'type': message_type,
            'content': content,
            'timestamp': int(time.time() * 1000)
        }
        return json.dumps(response)


# 测试函数
def test_message_handler():
    """测试消息处理器的功能"""
    handler = EchoKitMessageHandler()
    
    # 测试用例
    test_messages = [
        '[forward]',
        '[turn_left]',
        '{"type": "command", "command": "forward"}',
        '{"content": "[stop]"}',
        'forward',
        'invalid message',
        '[unknown_command]'
    ]
    
    print("Testing EchoKit Message Handler:")
    for msg in test_messages:
        result = handler.parse_message(msg)
        print(f"Input: {msg}")
        print(f"Output: {result}")
        print("-" * 50)


if __name__ == '__main__':
    test_message_handler()